#include <stdlib.h>
#include "usb_cam.h"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

cv::Mat YUYVToRGB(const unsigned char* yuyv_data, int width, int height) ;

int main(int argc, char* argv[])
{
    std::shared_ptr<usb_cam::UsbCam> app = std::make_shared<usb_cam::UsbCam>("/dev/video2", 4160, 3120, 10);
    app->set_exposure(false, 0, 0);
    app->configure();
    app->start();
    size_t len = app->get_image_size_in_bytes();
    char* buffer = new char[len];
    memset(&buffer[0], 0 ,sizeof(buffer));
    app->get_image(reinterpret_cast<char *>(&buffer[0]));

    FILE *fp = NULL;
    fp = fopen("output.jpg", "w+");
    if(fp != NULL) fwrite(reinterpret_cast<char *>(&buffer[0]),1,app->get_image_size_in_bytes(),fp);

    // yuvu
    // cv::Mat rgbImage = YUYVToRGB(yuyv_buffer, app->get_image_width(), app->get_image_height());
    
    // // 将RGB图像保存为JPEG文件
    // cv::imwrite("output.jpg", rgbImage);
    std::cout << "Image saved as output.jpg" << std::endl;
    delete[] buffer;
    return 0;
}
 
// 函数：将YUV422 (YUYV) 转换为 RGB
cv::Mat YUYVToRGB(const unsigned char* yuyv_data, int width, int height) 
{
    cv::Mat rgb(height, width, CV_8UC3);
    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j += 2) {
            int index = i * width * 2 + j *2;
            unsigned char Y0 = yuyv_data[index];
            unsigned char U = yuyv_data[index + 1];
            unsigned char Y1 = yuyv_data[index + 2];
            unsigned char V = yuyv_data[index + 3];
 
            // YUV to RGB conversion
            unsigned char R = cv::saturate_cast<unsigned char>(1.164f * (Y0 - 16) + 1.596f * (V - 128));
            unsigned char G = cv::saturate_cast<unsigned char>(1.164f * (Y0 - 16) - 0.813f * (V - 128) - 0.391f * (U - 128));
            unsigned char B = cv::saturate_cast<unsigned char>(1.164f * (Y0 - 16) + 2.018f * (U - 128));
            rgb.at<cv::Vec3b>(i, j) = cv::Vec3b(B, G, R);
 
            R = cv::saturate_cast<unsigned char>(1.164f * (Y1 - 16) + 1.596f * (V - 128));
            G = cv::saturate_cast<unsigned char>(1.164f * (Y1 - 16) - 0.813f * (V - 128) - 0.391f * (U - 128));
            B = cv::saturate_cast<unsigned char>(1.164f * (Y1 - 16) + 2.018f * (U - 128));
            rgb.at<cv::Vec3b>(i, j + 1) = cv::Vec3b(B, G, R);
        }
    }
    return rgb;
}

